// #####################################
// File     :LineFollower.c
// Author   : Chris Meaclem, Isaac Manuel
// Date     : 20/08/2011
// Release  : Intermediate
// Purpose  : Main method
// Notes    : 
// #####################################

#include <avr/io.h>
#include <avr/interrupt.h>
#include "motor.h"
#include "sensor.h"



//#define sei()

// Declare the current vehicle state of the vehicle. Use _Next to indicate a change in state is requested.
//Initially set to all STOP
	vechileState vechileState_Current = vechile_Stop;
	vechileState vechileState_Next = vechile_Stop;

void runMotorTest(void);
void testDriveState(vechileState s);
void pause(void);
void runSensorTest(void);
void testSensorState(vechileState s);

int main(void)
{
	int i = 0;
	
	motor_Init();  // Initilise the motor library and associated resources.
	setupRead();  // initialise the sensor library and associated resources. 
	//vechileState_Next = vechile_Rev;
	
	
	/*runMotorTest();
	pause();
	runSensorTest();*/
	
    while(1)
    {
		//sei();
		/*
		*	Dont make too many succesive calls to the vechile_Drive function. only call when a schange in state is required! Otherwise the wheel rotation is affected.
		*
		*	This should be one of the last functions called:
		*/
		vechileState_Next = getVehicleState();
		
		
		if(vechileState_Current != vechileState_Next)
		{
			// If there has been a request to change state, do so:
			vechile_Drive(vechileState_Next);
			vechileState_Current = vechileState_Next;  // Set current to next.
		}
		
		
		/* Delay Loop: */
		i = 0;
		while(i<1000)
		{
			i++;
		}
		/* END - Delay Loop: */
    }
	
	
	
	return 0;
}


/*
 * Test(left, right): forwards, left, right, forwards.
 */
void runSensorTest(void) {
	volatile int j = 0;
	
	// indicate that the function has been entered by
	// going into reverse for a small amount of time
	testDriveState(vechile_Rev);
	pause();
	
	testSensorState(vechile_For);    // (white, black)
	vechile_Drive(vechile_Stop);
	
	testSensorState(vechile_Left);   // (black, black)
	vechile_Drive(vechile_Stop);
	
	testSensorState(vechile_Right);  // (white, white)
	vechile_Drive(vechile_Stop);
	
	testSensorState(vechile_For);    // (black, white)
	vechile_Drive(vechile_Stop);
	
	// indicate that the function has been completed by
	// going into reverse for a small amount of time
	pause();
	testDriveState(vechile_Rev);
	pause();
	
	while (1) {
		j = 0;
		j++;
	}
}


void testSensorState(vechileState s) {
	vechileState current = vechile_Stop;
	
	while (current != s) {
		current = getVehicleState();
	}
	
	testDriveState(s);
}


/*
 * Test: forwards, left, right, reverse, forwards (forever!).
 */
void runMotorTest(void) {
	volatile unsigned int j = 0;
	
	testDriveState(vechile_For);
	pause();
	
	testDriveState(vechile_Left);
	pause();
	
	testDriveState(vechile_Right);
	pause();
	
	testDriveState(vechile_Rev);
	pause();
}

void testDriveState(vechileState s) {
	volatile unsigned int t = 0;
	
	vechile_Drive(s);
	
	while(t<20000) {
		t++;
	}
	
	//vechile_Drive(vechile_Stop);
}

void pause(void) {
	volatile unsigned int t = 0;
	
	vechile_Drive(vechile_Stop);
	
	while(t<10000) {
		t++;
	}
}

